#include "geometry.h"



Tvec Transform(Tvec v, Ttensor m)
{
   Tvec c;
   each(i) c.x[i] = sum(m[i], v.x);
   return c;
}

double FindRoot(Ffuncclbc *f, double x1, double x2, double prec)
{
    double x = -f(x1)*(x2-x1) / (f(x2)-f(x1)) + x1;
    if (Abs(f(x))<prec) return x; else
        if (Sgn(x)==Sgn(x1)) return FindRoot(f, x, x2, prec); else
                             return FindRoot(f, x, x1, prec);
    return 0;
}

int Roots(Ffuncclbc *f, int max_roots, double res, double prec, double x_min, double x_max, double roots[])
{
   int fnd = 0;                 // finded roots
   for (double x = x_min; x<=x_max-res; x += res)
   {
       int sgn1 = Sgn(f(x));
       int sgn2 = Sgn(f(x+res));
       if (sgn1 == 0) continue;     // this root has already been calculated
       if (sgn1 != sgn2)
       {
           if (sgn2 == 0)  roots[fnd++] = x+res; else
                           roots[fnd++] = FindRoot(f, x, x+res, prec);
           if (fnd == max_roots) goto end;
       }
   }
   end:
   roots[fnd] = 0;
   return fnd;
}

double Determinant(Ttensor t)
{
    //  | 00  10  20 |
    //  | 01  11  21 |
    //  | 02  12  22 |
    return (t[0][0]*t[1][1]*t[2][2] + t[1][0]*t[2][1]*t[0][2] + t[0][1]*t[1][2]*t[2][0] - t[2][0]*t[1][1]*t[0][2] - t[1][0]*t[0][1]*t[2][2] - t[2][1]*t[1][2]*t[0][0]);
}

Ttensor *tg;

void SwapVals(double &v1, double &v2)
{
    double v = v1;
    v1 = v2;
    v2 = v;
}

void SwapRows(Ttensor &t, int r1, int r2)
{
    each(i) SwapVals(t.t[r1][i], t.t[r2][i]);
}

void SumRows(Ttensor &t, int k, Tvec r)
{
    each(i) t.t[k][i] += r[i];
}

bool Gauss(Ttensor t, Tvec r, Tvec &sol)
{
    double k; 
    for (int i=0; i<3; i++) 
    {
		for (int j=0; j<3; j++)  
		if ((j<i && Abs(t.t[j][j])<ZERO) || j>i) if (Abs(t.t[i][i])<Abs(t.t[j][i]))
        {
	 	    SwapRows(t, i, j);
		    SwapVals(r.x[i], r.x[j]);
		}
		for (int j=0; j<3; j++) if (i!=j)
        {
            if (Abs(t.t[i][i])<ZERO) continue;
            k = -t.t[j][i]/t.t[i][i];
            SumRows(t, j, t[i] * k);
            r.x[i] = r.x[i] * k;
        }
    }
    // all rows are now null or they have first non-zero element on diagonal  
    for (int i=2; i>=0; i--)
    {
        if (t[i] < ZERO_COOR && t[i] > -ZERO_COOR)
        {
            if (r[i]!=0) return false;          // no solution
            else { sol.x[i] = 1; continue; }
        }
        sol.x[i] = r.x[i];
	for (int j=i+1; j<3; j++) sol.x[i] -= sol.x[j]*t[i][j];
	sol.x[i] /= t.t[i][i];

    }
    return true;
}

bool Gauss(Ttensor t, Ttensor r, Ttensor &sol)
{
    double k; 
    for (int i=0; i<3; i++) 
    {
		for (int j=0; j<3; j++)  
		if ((j<i && Abs(t.t[j][j])<ZERO) || j>i) if (Abs(t.t[i][i])<Abs(t.t[j][i]))
        {
 		    SwapRows(t, i, j);
		    SwapRows(r, i, j);
		}
		for (int j=0; j<3; j++) if (i!=j)
        {
            if (Abs(t.t[i][i])<ZERO) continue;
            k = -t.t[j][i]/t.t[i][i];
            SumRows(t, j, t[i] * k);
		    SumRows(r, j, r[i] * k);
        }
    }
    // all rows are now null or they have first non-zero element on diagonal  

    for (int i=2; i>=0; i--)
    {
		double k = t.t[i][i];
        if (t[i] < ZERO_COOR && t[i] > -ZERO_COOR)
		{
		    if (r[i] < ZERO_COOR && r[i] > -ZERO_COOR) continue; 
		    else return false;			// ex. (0 0 0 | 1 0 2)   => no solution
		}

		for (int j=0; j<3;  j++)
		{
		    t. t[i][j] /= k;	// to have 1 on diagonal
		    r.t[i][j] /= k;
		}
		for (int j=i-1; j>=0; j--) 
		{
		    k = -t.t[j][i];
		    SumRows(t,  j, t [i]*k);
		    SumRows(r, j, r[i]*k);
		}
    }
    sol = r;
    return true;
}




Ttensor Inverse(Ttensor t)
{
    Ttensor t1; 
    Gauss(t, ID, t1);
    return t1;
}

Ttensor Transpose(Ttensor t)
{
    Ttensor v;
    each(i) each(j) v.t[i][j] = t[j][i];
}


double DetPoly(double x)
{
    return Determinant(*tg - ID*x);
}


int Eigenvalues(Ttensor t, double vals[])
{
    tg = &t;
    return Roots(DetPoly, 3, 0.01, ZERO, -20, 20, vals);
}


int Eigenvectors(Ttensor t, Tvec vecs[])
{
    tg = &t;
    double vals[3];
    int fnd = Eigenvalues(t, vals);
    for (int i=0; i<fnd; i++) Gauss(t - ID*vals[i], NULL_COOR, vecs[i]);
    return fnd;
}
